#include "../include/WiimoteConnector.h"

//needed to access the eye_base and lookat_base
#include "../include/GLUI.h"
//float dotDistanceInMM = 8.5f * 25.4f;
//float screenHeightinMM = 20 * 25.4f;
float dotDistanceInMM = 100;
float screenHeightinMM = 300;
float radiansPerPixel = (float)(PI / 4) / 1024.0f; //45 degree field of view with a 1024x768 camera
float movementScaling = 8.0f;
float lookatScaling = 2.0f;
int m_dwWidth = 1024;
int m_dwHeight = 768;
float cameraVerticaleAngle = 0; //begins assuming the camera is point straight forward
float relativeVerticalAngle = 0; //current eye position view angle
bool cameraIsAboveScreen = false;//has no affect until zeroing and then is set automatically.

WiimoteConnector* WiimoteConnector::singleton = 0;

WiimoteConnector::WiimoteConnector(void)
{
	alpha = 0.3;
	pitch = new float[MAX_NUM_WIIMOTES];
	roll = new float[MAX_NUM_WIIMOTES];
	old_pitch = new float[MAX_NUM_WIIMOTES];
	old_roll = new float[MAX_NUM_WIIMOTES];
	eye = new Point3[MAX_NUM_WIIMOTES];
	lookat = new Point3[MAX_NUM_WIIMOTES];

	connected = new bool[MAX_NUM_WIIMOTES];
	wiimotes = new wiimote[MAX_NUM_WIIMOTES];
	WiimoteConnector::singleton = this;
	setup_connection(&wiimotes[0], 0);
	setup_connection(&wiimotes[1], 1);
}



WiimoteConnector::~WiimoteConnector(void)
{
	for(int i = 0; i < MAX_NUM_WIIMOTES; i++) {
		if(connected[i])
			wiimotes[i].Disconnect();
	}
}

bool WiimoteConnector::Connected(int wiimote)
{ return connected[wiimote]; }

bool WiimoteConnector::ButtonPressed(int wiimote, int button)
{ 
	if(button == WIIMOTE_BUTTON_A)
		return wiimotes[wiimote].Button.A();
}

bool WiimoteConnector::SeesLED(int wiimote, int LED)
{ return wiimotes[wiimote].IR.Dot[LED].bFound; }

void WiimoteConnector::Update()
{
	for(int i = 0; i < MAX_NUM_WIIMOTES; i++) {
		//if(!connected[i]) {
		//	setup_connection(&wiimotes[i], i);
		//}
		if(connected[i] && wiimotes[i].RefreshState() != NO_CHANGE)
		{	
			old_pitch[i] = pitch[i];
			old_roll[i] = roll[i];
			pitch[i] = (1 - alpha) * old_pitch[i] + alpha * wiimotes[i].Acceleration.Orientation.Pitch;
			roll[i] = (1 - alpha) * old_roll[i] + alpha * wiimotes[i].Acceleration.Orientation.Roll;
		}
	}
	
}



Point3 WiimoteConnector::GetEye(int wiimote)
{ return eye[wiimote]; }

Point3 WiimoteConnector::GetLookat(int wiimote)
{ return lookat[wiimote]; }


float WiimoteConnector::GetPitch(int wiimote)
{ return pitch[wiimote]; }


float WiimoteConnector::GetRoll(int wiimote)
{ return roll[wiimote]; }


void WiimoteConnector::UpdateIR(int wiimote_num,
								wiimote_state::ir::dot dot1, 
								wiimote_state::ir::dot dot2,
								wiimote_state::ir::dot dot3,
								wiimote_state::ir::dot dot4)
{
	Point2 firstPoint;
	Point2 secondPoint;
	int numvisible = 0;

	if (dot1.bFound)
	{
		wiimotePointsNormalized[0].x = 1.0 - dot1.X;
		wiimotePointsNormalized[0].y = dot1.Y;
		firstPoint.x = dot1.RawX;
		firstPoint.y = dot1.RawY;
		numvisible = 1;
	}
	if (dot2.bFound)
	{
		wiimotePointsNormalized[1].x = 1.0 - dot2.X;
		wiimotePointsNormalized[1].y = dot2.Y;
		if (numvisible == 0)
		{
			firstPoint.x = dot2.RawX;
			firstPoint.y = dot2.RawY;
			numvisible = 1;
		}
		else
		{
			secondPoint.x = dot2.RawX;
			secondPoint.y = dot2.RawY;
			numvisible = 2;
		}
	}
	if(dot3.bFound){
		wiimotePointsNormalized[2].x = 1.0 - dot3.X;
		wiimotePointsNormalized[2].y = dot3.Y;
		if (numvisible == 0)
		{
			firstPoint.x = dot3.RawX;
			firstPoint.y = dot3.RawY;
			numvisible = 1;
		}
		else if(numvisible==1)
		{
			secondPoint.x = dot3.RawX;
			secondPoint.y = dot3.RawY;
			numvisible = 2;
		}
	}

	if (dot4.bFound)
	{
		wiimotePointsNormalized[3].x = 1.0f - dot4.X;
		wiimotePointsNormalized[3].y = dot4.Y;
		if(numvisible==1)
		{
			secondPoint.x = dot4.RawX;
			secondPoint.y = dot4.RawY;
			numvisible = 2;
		}
	}

	if (numvisible == 2)
	{

		float dx = firstPoint.x - secondPoint.x;
		float dy = firstPoint.y - secondPoint.y;
		float pointDist = (float)sqrt(dx * dx + dy * dy);

		float angle = radiansPerPixel * pointDist / 2;
		//in units of screen hieght since the box is a unit cube and box hieght is 1
		eye[wiimote_num].z = Eye_base_z->get_float_val() + (movementScaling * (float)((dotDistanceInMM / 2) / tan(angle)) / screenHeightinMM);
		//handle the go-go portion - total hack
		if(wiimote_num == 1) {
			eye[wiimote_num].z = (eye[wiimote_num].z - 9.5) * 5;
		}

		float avgX = (firstPoint.x + secondPoint.x) / 2.0f;
		float avgY = (firstPoint.y + secondPoint.y) / 2.0f;
		eye[wiimote_num].x = Eye_base_x->get_float_val() - (((avgX - 512) * movementScaling) / 1024.0);
		//eye.y = Eye_base_y->get_float_val() - (((avgY - 334) * movementScaling) / 768.0);
		eye[wiimote_num].y = Eye_base_y->get_float_val();
		//should  calaculate based on distance

		lookat[wiimote_num].x = Lookat_base_x->get_float_val() + ((float)(lookatScaling *  sin(radiansPerPixel * (-avgX + 512)) * eye[wiimote_num].z));

		relativeVerticalAngle = (avgY - 384) * radiansPerPixel;//relative angle to camera axis

		if(cameraIsAboveScreen)
			lookat[wiimote_num].y = Lookat_base_y->get_float_val() - (.5f+(float)(lookatScaling * sin(relativeVerticalAngle + cameraVerticaleAngle)  * eye[wiimote_num].z));
		else
			lookat[wiimote_num].y = Lookat_base_y->get_float_val() - (-.5f + (float)(lookatScaling * sin(relativeVerticalAngle + cameraVerticaleAngle) * eye[wiimote_num].z));
	}

}


void WiimoteConnector::setup_connection(wiimote* wiimote_ptr, int wiimote_num)
{
	if(wiimote_ptr == 0)
		wiimote_ptr = new wiimote();
	if(!wiimote_ptr->Connect(wiimote::FIRST_AVAILABLE)) {
		this->connected[wiimote_num] = false;
	} else {
		this->connected[wiimote_num] = true;
		wiimote_ptr->SetLEDs(wiimote_num + 1);
		if(wiimote_ptr->bExtension)
			wiimote_ptr->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR_EXT); // no IR dots
		else
			wiimote_ptr->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR);
		
		wiimote_ptr->ChangedCallback		 = ir_change;
		wiimote_ptr->CallbackTriggerFlags = IR_CHANGED;
	}
}

int WiimoteConnector::GetWiimoteNumber(wiimote& wiimote)
{
	for(int i = 0; i < MAX_NUM_WIIMOTES; i++) {
		if(wiimote.LED.Lit(i))
			return i;
	}
}

void ir_change (wiimote &wiimote, state_change_flags changed)
{
	int i = WiimoteConnector::singleton->GetWiimoteNumber(wiimote);
	wiimote_state::ir::dot &dot1 = wiimote.IR.Dot[0];
	wiimote_state::ir::dot &dot2 = wiimote.IR.Dot[1];
	wiimote_state::ir::dot &dot3 = wiimote.IR.Dot[2];
	wiimote_state::ir::dot &dot4 = wiimote.IR.Dot[3];
	WiimoteConnector::singleton->UpdateIR(i, dot1, dot2, dot3, dot4);
}

